#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Utils.h>
#include <ceres/ceres.h>
#include <sophus/so3.hpp>
using namespace std;
using namespace Eigen;
namespace xio=lyxutils::io;
namespace xos=lyxutils::os;

void print_usage(){
    cout<<"usage:estimateAffine ref_pts.txt align_pts.txt [-o result.txt]"<<endl;
    cout<<"explain:esitimate the best affine transformation matrix between the given two point sets"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h              print this help message"<<endl;
    cout<<"  -o=result.txt   set output file name(default is result.txt)"<<endl;
}

struct PosErrorCost{//变换后点位置与参照点误差成本函数
    PosErrorCost(double x,double y,double z,double x_ref,double y_ref,double z_ref):
            _x(x),_y(y),_z(z),_x_ref(x_ref),_y_ref(y_ref),_z_ref(z_ref){}
    //残差计算
    bool operator()(const double *const A_t, double *residuals)const{
        //模型参数A_t,有12个，前九个是可逆矩阵元素,后三个是平移量
        Matrix3d R;
        R<<A_t[0],A_t[1],A_t[2],A_t[3],A_t[4],A_t[5],A_t[6],A_t[7],A_t[8];
        Vector3d X_align(_x,_y,_z);
        Vector3d t(A_t[9], A_t[10], A_t[11]);
        Vector3d X_trans=R*X_align+t;
        residuals[0]=_x_ref-X_trans[0];
        residuals[1]=_y_ref-X_trans[1];
        residuals[2]=_z_ref-X_trans[2];
        return true;
    }

    const double _x,_y,_z;
    const double _x_ref,_y_ref,_z_ref;
};

int main(int argc,char **argv) {
    xio::CLParser parser;
    try{
        parser.parse(argc,argv);
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
        print_usage();
        return 1;
    }
    if(argc<3||parser.hasOption("h")){
        print_usage();
        return 1;
    }
    vector<string> parameters=parser.getParameters();
    vector<vector<float> > ref_pts,align_pts;
    for(int i=0;i<2;++i)if(!xos::file(xos::abspath(parameters[i]))){
            cout<<"file:"<<parameters[i]<<" not exist!"<<endl;
            return 1;
        }//test file existence
    xio::read_csv(parameters[0],ref_pts,", ");
    xio::read_csv(parameters[1], align_pts,", ");
    string outputFile="result.txt";
    if(parser.hasOption("o")&&parser.getOptionArg("o")!=""){
        outputFile=parser.getOptionArg("o");
        if(xos::file(outputFile)){
            cout<<"output file:"<<outputFile<<" already exist,change to default:";
            outputFile="result.txt";
            cout<<outputFile<<endl;
        }
    }
    //do some real work
    //构建问题
    double A_t[12]={0};
    cout<<"初始参数:("<<A_t[0];
    for(int i=1;i<12;++i)cout << ","<<A_t[i];
    cout<<")"<<endl;
    ceres::Problem problem;
    int N=ref_pts.size()<align_pts.size()?ref_pts.size():align_pts.size();
    for(int i=0;i<N;++i){
        problem.AddResidualBlock(//自动求导,模板参数依次为成本函数类型,残差个数,参数个数
                new ceres::NumericDiffCostFunction<PosErrorCost,ceres::RIDDERS,3,12>(
                        new PosErrorCost(align_pts[i][0],align_pts[i][1],align_pts[i][2],
                                         ref_pts[i][0],ref_pts[i][1],ref_pts[i][2])
                ),
                nullptr,
                A_t
        );
    }
    //配置求解器
    ceres::Solver::Options options;
    options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout=true;//输出到cout
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);
    //输出结果
    cout<<summary.BriefReport()<<endl;
    Matrix3d A;
    A << A_t[0],A_t[1],A_t[2],A_t[3],A_t[4],A_t[5],A_t[6],A_t[7],A_t[8];
    Vector3d t(A_t[9], A_t[10], A_t[11]);
    cout<<"result:"<<endl;
    double RMS=sqrt(summary.final_cost/(N-1));
    cout<<"RMS="<<RMS<<endl;
    cout<<"t=("<<t(0)<<","<<t(1)<<","<<t(2)<<")"<<endl;
    cout<<"A="<<endl;
    cout << A << endl;
    cout<<"T="<<endl;
    Matrix4d T;
    T.topLeftCorner(3,3)=A;
    T.row(3)=Matrix<double,1,4>(0,0,0,1);
    T.topRightCorner(3,1)=t;
    cout<<T<<endl;
    ofstream ofs(outputFile);
    ofs<<setprecision(8);
    for(int i=0;i<T.rows();++i){
        ofs<<T(i,0);
        for(int j=1;j<T.cols();++j){
            ofs<<","<<T(i,j);
        }
        ofs<<endl;
    }
    ofs.flush();
    if(ofs.is_open())ofs.close();
    cout<<"result saved to file:"<<outputFile<<endl;
    return 0;
}